# -*- coding: utf-8 -*-
import numpy as np
import matplotlib.pyplot as plt
from scipy.interpolate import interp1d
from scipy.interpolate import CubicSpline
import matplotlib.font_manager as font_manager


# 自定义 三次差值，分段，斜率相同
def my_bezier_curve(points, num=100):
    """
    根据给定的点进行分段多次项曲线拟合
    :param points: 输入的点，形如 [(x1, y1), (x2, y2), (x3, y3)]
    :param num: 生成拟合曲线上的点的数量，默认为100
    :return: 拟合曲线上的点的坐标数组
    """
    # 将输入点转换为NumPy数组
    points = np.array(points)
    length = []
    for i in range(len(points)-1):
        length.append(np.sqrt((points[i+1][0]-points[i][0])**2 + (points[i+1][1]-points[i][1])**2))

    d_len = min(length)/2
    t = np.linspace(0, 1, num)
    x_all = []
    y_all = []

    # 获取第一个点和第三个点的坐标
    for index in range(len(points)-2):
        # 原始点
        p1 = points[index]
        p2 = points[index+1]
        p3 = points[index+2]

        # 减去d_len
        len1 = length[index]
        len2 = length[index+1]
        x1 = p2[0] - (p2[0]-p1[0])*d_len/len1
        y1 = p2[1] - (p2[1]-p1[1])*d_len/len1
        x2= p2[0]
        y2 =p2[1]
        x3 = p2[0] - (p2[0]-p3[0])*d_len/len2
        y3 = p2[1] - (p2[1]-p3[1])*d_len/len2

        x_list = []
        y_list = []
        for i in range(100):
            x = x1 * ((1 - t[i]) ** 2) + 2 * (1 - t[i]) * t[i] * x2 + x3 * t[i] ** 2
            y = y1 * ((1 - t[i]) ** 2) + 2 * (1 - t[i]) * t[i] * y2 + y3 * t[i] ** 2
            x_list.append(x)
            y_list.append(y)
        x_all = np.concatenate((x_all, x_list))
        y_all = np.concatenate((y_all, y_list))

    curve_points = np.column_stack((x_all, y_all))
    return curve_points


def quadratic_fit(points, num=100):
    """
    根据给定的点进行二次拟合
    :param points: 输入的点，形如 [(x1, y1), (x2, y2), ...]
    :param num: 生成拟合曲线上的点的数量，默认为100
    :return: 拟合曲线上的点的坐标数组
    """
    # 将输入点转换为NumPy数组
    points = np.array(points)
    # 进行二次拟合
    coeffs = np.polyfit(points[:, 0], points[:, 1], 2)
    # 生成拟合曲线上的点
    x_vals = np.linspace(points[0, 0], points[-1, 0], num)
    y_vals = np.polyval(coeffs, x_vals)
    # 将x和y坐标组合成新的点数组
    curve_points = np.column_stack((x_vals, y_vals))
    return curve_points


def cubic_fit(points, num=100):
    """
    根据给定的点进行三次拟合
    :param points: 输入的点，形如 [(x1, y1), (x2, y2), ...]
    :param num: 生成拟合曲线上的点的数量，默认为100
    :return: 拟合曲线上的点的坐标数组
    """
    # 将输入点转换为NumPy数组
    points = np.array(points)
    # 进行三次拟合
    coeffs = np.polyfit(points[:, 0], points[:, 1], 3)
    # 生成拟合曲线上的点
    x_vals = np.linspace(points[0, 0], points[-1, 0], num)
    y_vals = np.polyval(coeffs, x_vals)
    # 将x和y坐标组合成新的点数组
    curve_points = np.column_stack((x_vals, y_vals))
    return curve_points


def bezier_curve(points_, num=100):
    """
    根据给定的点生成二阶贝塞尔曲线
    :param points_: 输入的点，形如 [(x1, y1), (x2, y2), ...]
    :param num: 生成曲线上的点的数量，默认为100
    :return: 二阶贝塞尔曲线上的点的坐标数组
    """
    # 确保至少有3个点才能生成二阶贝塞尔曲线
    if len(points_) < 3:
        raise ValueError("至少需要3个点来生成二阶贝塞尔曲线")

    # 将输入点转换为NumPy数组
    points = np.array(points_)
    # 生成一个插值函数，使用二阶贝塞尔曲线插值
    curve_func = interp1d(points[:, 0], points[:, 1], kind='quadratic')
    # 生成num个点作为插值点
    x_vals = np.linspace(points[0, 0], points[-1, 0], num)
    # 使用插值函数计算对应的y值
    y_vals = curve_func(x_vals)
    # 将x和y坐标组合成新的点数组
    curve_points = np.column_stack((x_vals, y_vals))
    return curve_points


def spline_fit(points, num=100):
    """
    根据给定的点进行样条拟合
    :param points: 输入的点，形如 [(x1, y1), (x2, y2), ...]
    :param num: 生成拟合曲线上的点的数量，默认为100
    :return: 拟合曲线上的点的坐标数组
    """
    # 将输入点转换为NumPy数组
    points = np.array(points)
    # 进行样条拟合
    spline = CubicSpline(points[:, 0], points[:, 1])
    # 生成拟合曲线上的点
    x_vals = np.linspace(points[0, 0], points[-1, 0], num)
    y_vals = spline(x_vals)
    # 将x和y坐标组合成新的点数组
    curve_points = np.column_stack((x_vals, y_vals))
    return curve_points


# 示例用法
def test_use():
    # 原始的折线航迹点
    # original_points = [(0, 0), (7, 29), (10, 32), (15, 35.3), (16, 36), (16.3, 39), (19.5, 40.5),  (20, 43), (20.3, 47)]
    original_points = [(0, 0), (7, 29), (10, 32), (13, 33),  (16, 36), (16, 39), (20, 43), (20, 47)]
    # 生成二阶贝塞尔曲线上的点
    # smoothed_curve = bezier_curve(original_points)

    # # 生成二次拟合曲线上的点
    # quadratic_curve = quadratic_fit(original_points)
    # # 生成三次拟合曲线上的点
    # cubic_curve = cubic_fit(original_points)
    # # 生成样条拟合曲线上的点
    # spline_curve = spline_fit(original_points)
    # 生成分段拟合曲线上的点
    # piecewise_curve = piecewise_polynomial_fit(original_points)
    piecewise_curve = my_bezier_curve(original_points)
    np.save('npy/bezier_curve_50', np.array(piecewise_curve))

    # 将原始折线航迹和优化后的航迹画出来
    # 设置中文字体
    # plt.rcParams['font.sans-serif'] = ['SimHei']  # 设置中文字体为黑体
    plt.plot(*zip(*original_points), marker='o', color='blue', label='raw')
    # plt.plot(smoothed_curve[:, 0], smoothed_curve[:, 1], color='red', label='优化后航迹', linewidth=1)
    # plt.plot(quadratic_curve[:, 0], quadratic_curve[:, 1], marker='', color='green', label='二次拟合曲线')
    # plt.plot(cubic_curve[:, 0], cubic_curve[:, 1], marker='', color='red', label='三次拟合曲线')
    # plt.plot(spline_curve[:, 0], spline_curve[:, 1], marker='', color='red', label='样条拟合曲线')
    plt.plot(piecewise_curve[:, 0], piecewise_curve[:, 1], marker='', color='red', label='bezier')

    # 添加图例
    plt.legend()

    # 设置标题和轴标签
    plt.title('path smooth')
    plt.xlabel('x')
    plt.ylabel('y')

    # 显示图形
    plt.grid(True)
    plt.axis('equal')
    plt.show()


if __name__ == '__main__':
    test_use()
